#include "algorithm_bh.hpp"
#include "gen_struct.hpp"

void DrawGrid(PTNode node) {

	double lx = node -> ld_vertex.x,
	       ly = node -> ld_vertex.y,
	       lz = node -> ld_vertex.z;

	double rx = node -> ru_vertex.x,
	       ry = node -> ru_vertex.y,
	       rz = node -> ru_vertex.z;
	
	glBegin(GL_LINES);
		glVertex3f(lx, ly, lz); glVertex3f(rx, ly, lz);
		glVertex3f(rx, ly, lz); glVertex3f(rx, ly, rz);
		glVertex3f(rx, ly, rz); glVertex3f(lx, ly, rz);
		glVertex3f(lx, ly, rz); glVertex3f(lx, ly, lz);

		glVertex3f(lx, ry, lz); glVertex3f(rx, ry, lz);
		glVertex3f(rx, ry, lz); glVertex3f(rx, ry, rz);
		glVertex3f(rx, ry, rz); glVertex3f(lx, ry, rz);
		glVertex3f(lx, ry, rz); glVertex3f(lx, ry, lz);
	glEnd();
	glBegin(Gl_LINES);
	  	glVertex3f(lx,ly,lz); glVertex3f(lx,ry,lz);
	glEnd();
	glBegin(GL_LINES)
		glVertex3f(lx,ly,rz); glVertex3f(lx,ry,rz);
	glEnd();
	glBegin(GL_LINES);
		glVertex3f(rx,ly,lz); glVertex3f(rx,ry,lz);
	glEnd();
	glBegin(GL_LINES);
		glVertex3f(rx,ly,rz); glVertex3f(rx,ry,rz);
	glEnd();

	if ((node -> up_left_up) != NULL && ((node -> up_left_up -> size) != 0)) {
			DrawGrid(node -> up_left_up);
		}	
	if ((node -> up_left_down) != NULL && ((node -> up_left_down -> size) != 0)) {
			DrawGrid(node -> up_left_down);
		} 	
	if ((node -> up_right_up) != NULL && ((node -> up_right_up -> size) != 0)) {
			DrawGrid(node -> up_right_up);
		} 	
	if ((node -> up_right_down) != NULL && ((node -> up_right_down -> size) != 0)) {
			DrawGrid(node -> up_right_down);
		} 	
	if ((node -> down_left_up) != NULL && ((node -> down_left_up -> size) != 0)) {
			DrawGrid(node -> down_left_up);
		} 	
	if ((node -> down_left_down) != NULL && ((node -> down_left_down -> size) != 0)) {
			DrawGrid(node -> down_left_down);
		} 	
	if ((node -> down_right_up) != NULL && ((node -> down_right_up -> size) != 0)) {
			DrawGrid(node -> down_right_up);
		} 	
	if ((node -> down_right_down) != NULL && ((node -> down_right_down -> size) != 0)) {
			DrawGrid(node -> down_right_down);
		} 	
}


void InitGenInfo(gen_environ ** gen_struct_pr, int nParticles) {	//interface stuff
	gen_environ * gen_struct;

	gen_struct =  (gen_environ*)malloc(sizeof(gen_environ));
	gen_struct -> n_thr = 4;
	gen_struct -> thr_arr = (pthread_t*)malloc(sizeof(pthread_t) * (gen_struct -> n_thr));	

	gen_struct -> eps = 1e-3;	//minimal distance
	gen_struct -> G = 1;		//gravity constant
	gen_struct -> T = 0.3;		//accuracy parameter
	gen_struct -> dTime = 16;	
	gen_struct -> time_coeff = 1e-4;	//multiplication coeff
	gen_struct -> nParticles = nParticles;

	gen_struct -> treeRoot = (PTNode)malloc(sizeof(treeNode));
	gen_struct -> list = (CBody*)malloc(sizeof(CBody) * (gen_struct -> nParticles));
	gen_struct -> max_coord = 15;

	*gen_struct_pr = gen_struct;	
}
void DestroyAll(gen_environ * gen_struct) {	
    //TODO
    //avoid memory leaks
	for (int i = 0; i < gen_struct -> n_thr; ++i) {
		pthread_join(gen_struct -> thr_arr[i],NULL);
	}
	DestroyTree(gen_struct -> treeRoot);
	free(gen_struct -> thr_arr);
	free(gen_struct -> list);
	free(gen_struct);
}

/*type 1 - up_left_up
  type 2 - up_left_down
  type 3 - up_right_up
  type 4 - up_right_down
  type 5 - down_left_up
  type 6   down_left_down
  type 7   down_right_up
  type 8   down_right_down
*/
/*================Tree Workshop===================*/
void InitNode(PTNode parent_node, PTNode * pt_child_node, int type ) {

    PTNode child_node = (PTNode) malloc(sizeof(treeNode));  // Init root pointer
    
    child_node -> up_left_up      = NULL;
    child_node -> up_left_down    = NULL;
    child_node -> up_right_up     = NULL;
    child_node -> up_right_down   = NULL;
    child_node -> down_left_up    = NULL;
    child_node -> down_left_down  = NULL;
    child_node -> down_right_up   = NULL;
    child_node -> down_right_down = NULL;

    child_node -> size = 0;
    child_node -> body_id = -1;        //empty or it's not a leaf
    child_node -> mass = 0;
    child_node -> center_mass.x = 0;
    child_node -> center_mass.y = 0;
    child_node -> center_mass.z = 0;
      
    switch (type) {
         case 1 : //up_left_up
            child_node -> ld_vertex.x = parent_node -> ld_vertex.x;
            child_node -> ld_vertex.y = parent_node -> ld_vertex.y + 
			    ((parent_node -> ru_vertex.y) - (parent_node -> ld_vertex.y))/2;
	    child_node -> ld_vertex.z = parent_node -> ld_vertex.z;

            child_node -> ru_vertex.x = parent_node -> ld_vertex.x + 
                            ((parent_node -> ru_vertex.x) - (parent_node -> ld_vertex.x))/2;
            child_node -> ru_vertex.y = parent_node -> ru_vertex.y;
	    child_node -> ru_vertex.z = parent_node -> ld_vertex.z +
			    ((parent_node -> ru_vertex.z) - (parent_node -> ld_vertex.z))/2;
            break;
         case 2 : //up_left_down
            child_node -> ld_vertex.x = parent_node -> ld_vertex.x;
            child_node -> ld_vertex.y = parent_node -> ld_vertex.y;
	    child_node -> ld_vertex.z = parent_node -> ld_vertex.z;

            child_node -> ru_vertex.x = parent_node -> ld_vertex.x + 
                            ((parent_node -> ru_vertex.x) - (parent_node -> ld_vertex.x))/2;
            child_node -> ru_vertex.y = parent_node -> ld_vertex.y +
			    ((parent_node -> ru_vertex.y) - (parent_node -> ld_vertex.y))/2;
	    child_node -> ru_vertex.z = parent_node -> ld_vertex.z +
			    ((parent_node -> ru_vertex.z) - (parent_node -> ld_vertex.z))/2;
			 
            break;
          case 3 : //up_right_up
            child_node -> ld_vertex.x = parent_node -> ld_vertex.x +
			    ((parent_node -> ru_vertex.x) - (parent_node -> ld_vertex.x))/2;
            child_node -> ld_vertex.y = parent_node -> ld_vertex.y +
			    ((parent_node -> ru_vertex.y) - (parent_node -> ld_vertex.y))/2;
	    child_node -> ld_vertex.z = parent_node -> ld_vertex.z;	  

            child_node -> ru_vertex.x = parent_node -> ru_vertex.x;
            child_node -> ru_vertex.y = parent_node -> ru_vertex.y; 
	    child_node -> ru_vertex.z = parent_node -> ld_vertex.z +
			    ((parent_node -> ru_vertex.z) - (parent_node -> ld_vertex.z))/2;
            break;
          case 4 : //up_right_down
            child_node -> ld_vertex.x = parent_node -> ld_vertex.x + 
                            ((parent_node -> ru_vertex.x) - (parent_node -> ld_vertex.x))/2;
            child_node -> ld_vertex.y = parent_node -> ld_vertex.y; 
	    child_node -> ld_vertex.z = parent_node -> ld_vertex.z;

            child_node -> ru_vertex.x =  parent_node -> ru_vertex.x;
            child_node -> ru_vertex.y =  parent_node -> ru_vertex.y +
			    ((parent_node -> ru_vertex.y) - (parent_node -> ld_vertex.y))/2;
	    child_node -> ru_vertex.z = parent_node -> ld_vertex.z +
			    ((parent_node -> ru_vertex.z) - (parent_node -> ld_vertex.z))/2;
            break;
	  case 5 : //down_left_up
            child_node -> ld_vertex.x = parent_node -> ld_vertex.x;
            child_node -> ld_vertex.y = parent_node -> ld_vertex.y + 
			    ((parent_node -> ru_vertex.y) - (parent_node -> ld_vertex.y))/2;
	    child_node -> ld_vertex.z = parent_node -> ld_vertex.z +
			    ((parent_node -> ru_vertex.z) - (parent_node -> ld_vertex.z))/2;

            child_node -> ru_vertex.x = parent_node -> ld_vertex.x + 
                            ((parent_node -> ru_vertex.x) - (parent_node -> ld_vertex.x))/2;
            child_node -> ru_vertex.y = parent_node -> ru_vertex.y;
	    child_node -> ru_vertex.z = parent_node -> ru_vertex.z;
            break; 
	  case 6 : //down_left_down
            child_node -> ld_vertex.x = parent_node -> ld_vertex.x;
            child_node -> ld_vertex.y = parent_node -> ld_vertex.y;
	    child_node -> ld_vertex.z = parent_node -> ld_vertex.z +
			    ((parent_node -> ru_vertex.z) - (parent_node -> ld_vertex.z))/2;

            child_node -> ru_vertex.x = parent_node -> ld_vertex.x + 
                            ((parent_node -> ru_vertex.x) - (parent_node -> ld_vertex.x))/2;
            child_node -> ru_vertex.y = parent_node -> ld_vertex.y +
			    ((parent_node -> ru_vertex.y) - (parent_node -> ld_vertex.y))/2;
	    child_node -> ru_vertex.z = parent_node -> ru_vertex.z;
	    break;
	  case 7 : //down_right_up
            child_node -> ld_vertex.x = parent_node -> ld_vertex.x +
			    ((parent_node -> ru_vertex.x) - (parent_node -> ld_vertex.x))/2;
            child_node -> ld_vertex.y = parent_node -> ld_vertex.y +
			    ((parent_node -> ru_vertex.y) - (parent_node -> ld_vertex.y))/2;
	    child_node -> ld_vertex.z = parent_node -> ld_vertex.z + 
			    ((parent_node -> ru_vertex.z) - (parent_node -> ld_vertex.z))/2;	  

            child_node -> ru_vertex.x = parent_node -> ru_vertex.x;
            child_node -> ru_vertex.y = parent_node -> ru_vertex.y; 
	    child_node -> ru_vertex.z = parent_node -> ru_vertex.z;
	    break;
	  case 8 : //down_right_down
            child_node -> ld_vertex.x = parent_node -> ld_vertex.x + 
                            ((parent_node -> ru_vertex.x) - (parent_node -> ld_vertex.x))/2;
            child_node -> ld_vertex.y = parent_node -> ld_vertex.y; 
	    child_node -> ld_vertex.z = parent_node -> ld_vertex.z +
			    ((parent_node -> ru_vertex.z) - (parent_node -> ld_vertex.z))/2;

            child_node -> ru_vertex.x =  parent_node -> ru_vertex.x;
            child_node -> ru_vertex.y =  parent_node -> ru_vertex.y +
			    ((parent_node -> ru_vertex.y) - (parent_node -> ld_vertex.y))/2;
	    child_node -> ru_vertex.z = parent_node -> ru_vertex.z;
	    break;
       }
    *pt_child_node = child_node; 
}

bool IsInsideNode(CPoint position, PTNode node) {    //check if the vertex is inside the box
           if (position.x <= (node -> ru_vertex.x) && position.x >= (node -> ld_vertex.x) 
            && position.y <= (node -> ru_vertex.y) && position.y >= (node -> ld_vertex.y)
	    && position.z <= (node -> ru_vertex.z) && position.z >= (node -> ld_vertex.z))
    	   return true; 
    	   return false;  
}
PTNode GetNode(PTNode node, CBody particle) {
    
    if (IsInsideNode(particle.position,node -> up_left_up)) {
        return (node -> up_left_up);
    }
    if (IsInsideNode(particle.position,node -> up_left_down)) {
        return (node -> up_left_down);
    }
    if (IsInsideNode(particle.position, node -> up_right_up)) {
        return (node -> up_right_up);
    }
    if (IsInsideNode(particle.position,node -> up_right_down)) {
        return (node -> up_right_down);
    }
    if (IsInsideNode(particle.position,node -> down_left_up)) {
        return (node -> down_left_up);
    }
    if (IsInsideNode(particle.position,node -> down_left_down)) {
        return (node -> down_left_down);
    }
    if (IsInsideNode(particle.position,node -> down_right_up)) {
        return (node -> down_right_up);
    }
    if (IsInsideNode(particle.position,node -> down_right_down)) {
        return (node -> down_right_down);
    }
    return NULL;		//in case of an error
} //Find the node among children where the particle is
void DestroyTree(PTNode root)
{
  if (root != NULL)
  {
    if (root -> size > 1)
        {
            DestroyTree(root->up_left_up);
            DestroyTree(root->up_left_down);
            DestroyTree(root->up_right_up);
            DestroyTree(root->up_right_down);
            DestroyTree(root->down_left_up);
            DestroyTree(root->down_left_down);
            DestroyTree(root->down_right_up);
            DestroyTree(root->down_right_down);
        }
    //releasing only  structure - ptnode
     free(root);
  }     
}
void BuildTree(gen_environ * gen_struct) {

    DestroyTree(gen_struct -> treeRoot);
    
    gen_struct -> treeRoot = (PTNode) malloc(sizeof(treeNode));   // Init root pointer
    gen_struct -> treeRoot -> size = 0;
    
    gen_struct -> treeRoot -> mass = 0;
    gen_struct -> treeRoot -> center_mass.x = 0;
    gen_struct -> treeRoot -> center_mass.y = 0;
    gen_struct -> treeRoot -> center_mass.z = 0;
    gen_struct -> treeRoot -> ld_vertex.x = -(gen_struct -> max_coord);
    gen_struct -> treeRoot -> ld_vertex.y = -(gen_struct -> max_coord);
    gen_struct -> treeRoot -> ld_vertex.z = -(gen_struct -> max_coord);
    gen_struct -> treeRoot -> ru_vertex.x =  (gen_struct -> max_coord);
    gen_struct -> treeRoot -> ru_vertex.y =  (gen_struct -> max_coord);
    gen_struct -> treeRoot -> ru_vertex.z =  (gen_struct -> max_coord);
    
    for (int i = 0; i < (gen_struct -> nParticles); ++i)
    {
        InsertParticle(gen_struct -> treeRoot, gen_struct -> list, i);
    }
}

void InsertParticle(PTNode node, CBody * list, int particle_id){
    
    if (node -> size > 1) {              //it's not a leaf
        PTNode subnode = GetNode(node, list[particle_id]);
        InsertParticle(subnode, list, particle_id);
    }
    if ((node -> size) == 1) {              //it's a leaf
        InitNode(node,&node->up_left_up,1); //making subdivision
        InitNode(node,&node->up_left_down,2);
        InitNode(node,&node->up_right_up,3);
        InitNode(node,&node->up_right_down,4);
        InitNode(node,&node->down_left_up,5);
        InitNode(node,&node->down_left_down,6);
        InitNode(node,&node->down_right_up,7);
        InitNode(node,&node->down_right_down,8);
        
        PTNode move_subnode = GetNode(node,list[node -> body_id]);
        InsertParticle(move_subnode, list, node -> body_id);     //copy the old particle
        
        node -> body_id = -1;
        
        PTNode subnode = GetNode(node,list[particle_id]);    //put a new particle
        InsertParticle(subnode,list,particle_id);
        
    }
    if (node -> size == 0) {             //yup, a leaf, but empty one
        node -> body_id = particle_id;
    }
    ++(node -> size);
}
/*====================================*/
void ComputeMassDistribution(PTNode node, CBody * list) {
    if (node -> size == 1) {
	node -> center_mass(list[node -> body_id].position);
        node->mass = list[node->body_id].mass;
    }
    if (node -> size == 0) {
        node -> center_mass.set(0);
	node -> mass = 0;
    }
    if (node -> size > 1)
    {
       ComputeMassDistribution(node->up_left_up,list);
       ComputeMassDistribution(node->up_left_down,list);
       ComputeMassDistribution(node->up_right_up,list);
       ComputeMassDistribution(node->up_right_down,list);
       ComputeMassDistribution(node->down_left_up,list);
       ComputeMassDistribution(node->down_left_down,list);
       ComputeMassDistribution(node->down_right_up,list);
       ComputeMassDistribution(node->down_right_down,list);
       
       node->mass = node->up_left_up->mass + node->up_left_down->mass + node->up_right_up->mass +       //mass in the node
                    node->up_right_down->mass + node->down_left_up->mass + node->down_left_down->mass +
		    node->down_right_up->mass + node->down_right_down->mass;

       node->center_mass = ((node->up_left_up->center_mass * node->up_left_up->mass) +    //x coord. up center 
                             (node->up_left_down->center_mass * node->up_left_down->mass) + 
                             (node->up_right_up->center_mass * node->up_right_up->mass) + 
                             (node->up_right_down->center_mass * node->up_right_down->mass)+
       			     (node->down_left_up->center_mass * node->down_left_up->mass) +    //x coord. down center 
                             (node->down_left_down->center_mass * node->down_left_down->mass) + 
                             (node->down_right_up->center_mass * node->down_right_up->mass) + 
                             (node->down_right_down->center_mass * node->down_right_down->mass))/(node->mass);
                             
    }
}
void MoveBodies(CBody * list, gen_environ * gen_struct)
{
    CPoint deltav;   //dv = f/m * DT
    CPoint deltar;   //dr = (v + dv/2) * DT
    int nBodies = gen_struct -> nParticles;
    double dTime = (gen_struct -> dTime) * (gen_struct -> time_coeff);    

    for (int i = 0; i < nBodies;++i)
    {	
	deltav = list[i].grForce * dTime;
        deltar.x = (list[i].vel + (deltav/2)) * dTime;
        
	list[i].vel = list[i].vel + deltav;

        list[i].position.x += deltar.x;
            if (fabs(list[i].position.x) > (gen_struct -> max_coord))
                    gen_struct -> max_coord = fabs(list[i].position.x) + 1;
                    
        list[i].position.y += deltar.y;
            if (fabs(list[i].position.y) > (gen_struct -> max_coord))
                    gen_struct -> max_coord = fabs(list[i].position.y) + 1;
	
	list[i].position.z += deltar.z;
	    if (fabs(list[i].position.z) > (gen_struct -> max_coord))
		    gen_struct -> max_coord = fabs(list[i].position.z) + 1;
        
	list[i].grForce.set(0);            
        
    }
}
void EvaluateTreeForces(PTNode node, CBody * list, int body_id, gen_environ * gen_struct)
{
    if (((node->size) == 1) && ((node -> body_id) != body_id)) {
        double r = (node->center_mass - list[body_id].position).magnitude();
        if (r > (gen_struct -> eps)) {                
            list[body_id].grForce = list[body_id].grForce + 
			((list[body_id].mass)*(node->mass)*(node->center_mass - list[body_id].position)/(r*r*r));
        } else {
	    double eps = gen_struct -> eps;
	    list[body_id].grForce = list[body_id].grForce + 
			((list[body_id].mass)*(node->mass)*(node->center_mass - list[body_id].position)/(eps*eps*eps));
            
        }
    }
    if (node->size > 1)  {
        double r = (node->center_mass.x - list[body_id].position).magnitude();
        double d = pow(node->ru_vertex.x - node->ld_vertex.x,2);                //height of the node
        double t = d / (r * r);		//solid angle
        if (t < (gen_struct -> T)) 
            {
                list[body_id].grForce.x += (list[body_id].mass) * (node->mass) * (node->center_mass.x - list[body_id].position.x)/(r*r*r);
                list[body_id].grForce.y += (list[body_id].mass) * (node->mass) * (node->center_mass.y - list[body_id].position.y)/(r*r*r);
                list[body_id].grForce.z += (list[body_id].mass) * (node->mass) * (node->center_mass.z - list[body_id].position.z)/(r*r*r);
            }
        else 
            {
                EvaluateTreeForces(node->up_left_up, list, body_id, gen_struct);
                EvaluateTreeForces(node->up_left_down, list, body_id, gen_struct);
                EvaluateTreeForces(node->up_right_up, list, body_id, gen_struct);
                EvaluateTreeForces(node->up_right_down, list, body_id, gen_struct);
                EvaluateTreeForces(node->down_left_up, list, body_id, gen_struct);
                EvaluateTreeForces(node->down_left_down, list, body_id, gen_struct);
                EvaluateTreeForces(node->down_right_up, list, body_id, gen_struct);
                EvaluateTreeForces(node->down_right_down, list, body_id, gen_struct);
            }
    }
}

/*========================================================================*/		//thread part for calculations
void *TreeForces(void *args) {	//simple thread function

  int n1 = ((thr_arg*)args) -> first;
  int n2 = ((thr_arg*)args) -> last;
  PTNode treeRoot = ((thr_arg*)args ) -> gen_struct -> treeRoot;
  body * list = ((thr_arg*)args) -> gen_struct -> list;

  for (int i = n1; i < n2; ++i)
 	 EvaluateTreeForces(treeRoot,list,i,((thr_arg*)args)-> gen_struct);
}

void CalculateForces(gen_environ * gen_struct) { //thread controller
 thr_arg  arg[gen_struct -> n_thr];

 int i;
 for ( i = 0;  i < gen_struct -> n_thr; ++i) {
	arg[i].first = (i * (gen_struct -> nParticles)) / (gen_struct -> n_thr);
	arg[i].last = ((i + 1) * (gen_struct -> nParticles)) / (gen_struct -> n_thr);
	arg[i].gen_struct = gen_struct;
	pthread_create(&(gen_struct -> thr_arr[i]),NULL,TreeForces,(void *)(&arg[i]));
 }
 for ( i = 0; i < gen_struct -> n_thr; ++i) 
 	pthread_join(gen_struct -> thr_arr[i],NULL);
    //TODO
    //this is the shit -- creating and deleting threads is stupid -- use semget, semctl and so on
}
